/*
 * Name       : prosper_user_code.cpp
 * Author     : Xiaoafei
 * Copyright  : 2017 The Prosper Tech
 *
 * Brief      : 成都普诺思博科技有限公司通用移动平台上层通信接口 v1.0
 */
#include "prosper_user_code.h"
#include <unistd.h>

unsigned int g_fdCom = 0;
std::string serial_dev;

namespace prosper_user {

    ProsperUser::ProsperUser(std::string name) :
            driver_name_(name),
            RobotDriverBase(name),
            nh_(){
        serial_dev = nh_private_.param("dev_port",std::string("/dev/ttyUSB0"));

        //TrailerModeCtl(1,0.7,0.6);
        ros::NodeHandle nh_p = ros::NodeHandle("~");
        nh_private_ = ros::NodeHandle(nh_p, name);

        trailer_ = nh_p.param("trailer",1);
        arm_working_ = true;
        arm_working_th_ = boost::thread(boost::bind(&ProsperUser::arm_working_exec,this));

        laser_sub_ = nh_.subscribe("/scan",20,&ProsperUser::laserposeCallBack, this);

        /*订阅里程计*/
        odom_sub_ = nh_.subscribe("odom", 1, &ProsperUser::odomCallBack,this);
        /*订阅世界坐标*/
        world_pose_sub_ = nh_.subscribe("robot_pose", 1, &ProsperUser::worldPoseUpdateCallBack,this);
        /*订阅紧急消息*/
        emergent_info_sub_ = nh_.subscribe("emergent_info", 1, &ProsperUser::emergentInfoCallBack,this);
        /*订阅机器人状态消息*/
        robot_status_sub_ = nh_.subscribe("robot_status", 1, &ProsperUser::robotStatusInfoCallBack,this);
        /*订阅机器人自定义任务*/
        self_def_task_info_sub_ = nh_.subscribe("self_def_task_info", 1, &ProsperUser::selfDefTaskInfoCallback,this);
        /*订阅机器人位置调整完成响应*/
        pose_ctl_task_status_info_sub_ = nh_.subscribe("pose_ctl_task_status_info", 1, &ProsperUser::poseCtlTaskStatusInfoCallback,this);

        /*发布底盘速度控制消息*/
        vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
        /*任务执行结果反馈*/
        task_status_pub_ = nh_.advertise<prosper_underpan::task_status_info>("task_status_info", 1);
        /*机器人位置调整任务*/
        pose_ctl_task_pub_ = nh_.advertise<prosper_underpan::pose_ctl_task_info>("pose_ctl_task_info", 1);
        /*发布机器人主任务*/
        robot_main_task_pub_ = nh_.advertise<prosper_underpan::main_task_info>("charging_task_info", 1);
    

        //ROS_INFO("trailer_::%d",trailer_);
        if(trailer_==1){
            WriteTrailerStatusToYaml(1,1.1,0.65);
        }
        else if(trailer_==2){
            WriteTrailerStatusToYaml(1,0.65,0.4);
        }
    }

    ProsperUser::~ProsperUser() {
      arm_working_ = false;
      arm_working_th_.join();
    }


    void ProsperUser::laserposeCallBack(const sensor_msgs::LaserScanConstPtr& laser){
      std::vector<long> laser_data;
      for(int i=0;i<laser->ranges.size();i++)
      {
        if(laser->ranges[i] > 0.4 && laser->ranges[i] < 6.0){
          laser_data.push_back(long(laser->ranges[i] * 1000));
        }else{
          laser_data.push_back(0.0);
        }
      }
      stamp_ = laser->header.stamp;
      frame_id_ = laser->header.frame_id;

      tf::StampedTransform transform_laser;
      tf::Transform laser_base1;

      tf::Vector3 point;
      tf::Vector3 point_base;
      std::vector<tf::Vector3> points_base;

         if(listener_.waitForTransform("map",frame_id_,ros::Time(0),ros::Duration(0.2))){
           listener_.lookupTransform("map",frame_id_,ros::Time(0),transform_laser);

           Eigen::Isometry3d t1 = Eigen::Isometry3d::Identity();
           double* dt1 =  t1.data();

           dt1[0]=transform_laser.getBasis()[0][0];
           dt1[1]=transform_laser.getBasis()[1][0];
           dt1[2]=transform_laser.getBasis()[2][0];
           dt1[3]=0;
           dt1[4]=transform_laser.getBasis()[0][1];
           dt1[5]=transform_laser.getBasis()[1][1];
           dt1[6]=transform_laser.getBasis()[2][1];
           dt1[7]=0;
           dt1[8]=transform_laser.getBasis()[0][2];
           dt1[9]=transform_laser.getBasis()[1][2];
           dt1[10]=transform_laser.getBasis()[2][2];
           dt1[11]=0;
           dt1[12]=transform_laser.getOrigin().x();
           dt1[13]=transform_laser.getOrigin().y();
           dt1[14]=0;
           dt1[15]=1;

           for(int i=0;i<laser_data.size();i++){
             //float angle=3.14159*(((float)180/721)*(float)i)/180.0;
             float angle=3.14159*(-24.6+0.00436*180/3.14*(float)i)/180.0;

             Eigen::Vector4d trans1;
             double *tran=trans1.data();
             tran[0]=laser_data[i]*sin(angle)/1000;
             tran[1]=-laser_data[i]*cos(angle)/1000;
             tran[2]=0;
             tran[3]=1;
             Eigen::Vector4d pose=t1*trans1;

             tf::Vector3 aa;
             aa.setX(pose[0]);
             aa.setY(pose[1]);
             aa.setZ(0);

//             tf::Vector3 cc;
//             cc = global_pose_.inverse()*aa;

             points_base.push_back(aa);
           }
           if(!robot_laser_pose_callback_.empty()){
             robot_laser_pose_callback_(points_base);
           }
         }

    }



/*****************************************************************************************************
 * Function:       // PubVel
 * Description:    // 底盘速度控制接口
 * Publisher:      // vel_pub
 * Topic:          // "cmd_vel"
 * Input:          // double vx: 底盘x轴速度(前进后退，前进为证，范围：-1.5m/s~1.5m/s)
 *                 // double vy: 底盘y轴速度(左右平移，向左为正，范围：-1.5m/s~1.5m/s)
 *                 // double vth:底盘z轴旋转速度(绕中心旋转，逆时针为正,范围：-1.57rad/s~1.57rad/s)
 * Output:         // none
 * Return:         // none
 * Notice:         // 速度控制大小应在合理范围内；一条速度指令最多持续150ms,如果要连续控制，两帧速度命令至少间隔100ms
*******************************************************************************************************/
    bool ProsperUser::pubVel(float vx, float vy, float vth) {
        geometry_msgs::Twist vel_cmd;
        vel_cmd.linear.x = vx;
        vel_cmd.linear.y = vy;
        vel_cmd.angular.z = vth;
        vel_pub_.publish(vel_cmd);
        return true;
    }

/*****************************************************************************************************
 * Function:       // odomCallBack
 * Description:    // 里程计回调函数，用于得到从底盘上电开始，机器人利用内部传感器推算出的在环境的坐标
 * Subscriber:     // odom_sub
 * Topic:          // "odom"
 * Input:          // nav_msgs::Odometry::ConstPtr &msg: 订阅得到的里程计消息
 * Output:         // none
 * Return:         // none
 * Notice:         // 里程计在地毯环境、底盘打滑等情况下可能误差较大
*******************************************************************************************************/
    void ProsperUser::odomCallBack(const nav_msgs::Odometry::ConstPtr &msg) {
//    double odom_pose[3];
//    odom_pose[0] = msg->pose.pose.position.x;
//    odom_pose[1] = msg->pose.pose.position.y;
//    odom_pose[2] = angle_normalize(tf::getYaw(msg->pose.pose.orientation));

        if (!odom_callback_.empty()) {
            odom_callback_(msg);
        }
    }

/*****************************************************************************************************
 * Function:       // worldPoseUpdateCallback
 * Description:    // 机器人世界坐标回调函数，机器人在定位状态下会回调此函数，得到机器人的世界坐标
 * Subscriber:     // world_pose_sub
 * Topic:          // "robot_pose"
 * Input:          // prosper_underpan::robot_pose_info::ConstPtr& msg: 位置消息
 * Output:         // none
 * Return:         // none
 * Notice:         // 机器人世界坐标一般只在定位的成功的情况下有用，机器人在地图上定位完成后，会不断输出在环境中的真实位置
*******************************************************************************************************/
    void ProsperUser::worldPoseUpdateCallBack(const prosper_underpan::robot_pose_info::ConstPtr& msg) {
        double world_pose[3];
        world_pose[0] = msg->x;
        world_pose[1] = msg->y;
        world_pose[2] = angle_normalize(msg->th);
        geometry_msgs::Pose pose;

        pose.position.x = msg->x;
        pose.position.y = msg->y;
        pose.position.z = 0;
        tf::Quaternion q;
        q.setRotation(tf::Vector3(0, 0, 1), world_pose[2]);
        pose.orientation.x = q.x();
        pose.orientation.y = q.y();
        pose.orientation.z = q.z();
        pose.orientation.w = q.w();
        if (!world_pose_callback_.empty()) {
            world_pose_callback_(pose);
        }
    }


/*****************************************************************************************************
 * Function:       // robotStatusInfoCallBack
 * Description:    // 机器人状态信息回调
 *                 //
 *                 // ##############机器人状态信息#############
 *                 // #
 *                 // # string robot_id
 *                 // # bool is_online       #群控连接情况
 *                 // # int32 mode           #运行模式
 *                 // # bool is_relocated    #是否重定位
 *                 // # uint32 map_id        #所处地图id
 *                 // # float32 pos_x        #机器人坐标
 *                 // # float32 pos_y        #机器人坐标
 *                 // # float32 pos_theta    #机器人坐标
 *                 // # string task_name     #当前任务
 *                 // # int32 task_progress  #任务进度
 *                 // # bool is_charging     #是否在充电
 *                 // # int32 battery        #电量
 *                 // # bool emergency       #是否急停
 *                 // # int32 error_code     #任务错误代码
 *                 // #
 *                 // #########################################
 *                 //
 * Subscriber:     // robot_status_sub
 * Topic:          // "robot_status"
 * Input:          // prosper_underpan::robot_status_info::ConstPtr& msg: 状态消息
 * Output:         // none
 * Return:         // none
 * Notice:         // 机器人开机后会不断输出该消息，反映机器人的实时状态
*******************************************************************************************************/
    void ProsperUser::robotStatusInfoCallBack(const prosper_underpan::robot_status_info::ConstPtr& msg){
      if(!robot_status_info_callback_.empty()){
        RobotStatusInfo info;
        info.battery = msg->battery;
        info.emergency = msg->emergency;
        info.error_code = msg->error_code;
        info.is_charging = msg->is_charging;
        info.is_online = msg->is_online;
        info.is_relocated = msg->is_relocated;
        info.map_id = msg->map_id;
        info.mode = msg->mode;
        info.pos_theta = msg->pos_theta;
        info.pos_x = msg->pos_x;
        info.pos_y = msg->pos_y;
        info.robot_id = msg->robot_id;
        info.task_name = msg->task_name;
        info.task_progress = msg->task_progress;
        robot_status_info_callback_(info);
      }
    }

/*****************************************************************************************************
 * Function:       // SendPoseAdjustRequest
 * Description:    // 向机器人发送位置调整的请求，请求机器人以某个相对于当前位置的相对姿态调节自身位置
 *                 // 例如:让机器人相对于当前位置前进1m并左旋30度,机器人调节完成后将发出“pose_ctl_task_status_info”消息响应
 * Publisher:      // pose_ctl_task_pub
 * Topic:          // "pose_ctl_task_info"
 * Input:          // int pose_type:       1-基于里程计，借助底盘原始数据达到位置调节的功能，遥控和巡航模式通用
 *                 //                      2-基于SLAM，借助修正后的世界坐标调节机器人位置，一般用于巡航模式
 *                 // bool switch_x:       机器人x，即前后方向是否做调整 是-true 否-false
 *                 // bool switch_y:       机器人y，即左右方向是否做调整 是-true 否-false
 *                 // bool switch_th:      机器人z，即角度是否做调整 是-true 否-false
 *                 // double x_set:        x轴相对移动量(单位：m) +相对当前位置前进 -相对当前位置后退
 *                 // double y_set:        y轴相对移动量(单位：m) +相对当前位置左平移 -相对当前位置右平移
 *                 // double th_set:       z轴相对旋转量(单位：rad) +相对当前位置逆时针旋转 -相对当前位置顺时针旋转
 *                 // double max_vel_x:    x轴调整最大线速度(单位：m/s)  正值
 *                 // double max_vel_y:    y轴调整最大线速度(单位：m/s)  正值
 *                 // double max_vel_th:   z轴调整最大角速度(单位：rad/s) 正值
 *                 // double precision_x:  x轴调节精度(单位：m) 正值  基于里程计：一般最小0.003  基于世界坐标：一般最小0.008
 *                 // double precision_y:  y轴调节精度(单位：m) 正值  基于里程计：一般最小0.003  基于世界坐标：一般最小0.008
 *                 // double precision_th: z轴调节精度(单位：rad) 正值  基于里程计：一般最小0.1＊pi/180  基于世界坐标：一般最小0.25*pi/180
 * Output:         // none
 * Return:         // none
 * Notice:         // 如果只调节一个或者两个轴，推荐将不调节的轴(switch)关闭
 *                 // 由于传感器精度的原因，机器人位置调整精度不宜过高，否则容易引起震荡
 *                 // 机器人完成位置调整后会有“PoseAck”响应，用户收到响应后即可进行下一步操作
*******************************************************************************************************/
    bool ProsperUser::sendPoseAdjustRequest(int request_code,
                                            bool switch_x, bool switch_y, bool switch_th,
                                            double x_set, double y_set, double th_set,
                                            double max_vel_x, double max_vel_y, double max_vel_th,
                                            double precision_x, double precision_y, double precision_th) {
//ROS_INFO("DDDDDDDD");
      prosper_underpan::pose_ctl_task_info pose_request;

      unsigned int pose_ctl_time_stamp = ros::Time::now().toSec()*1000;
      std::string pose_ctl_task_name = "user_define";

      pose_request.time_stamp = pose_ctl_time_stamp;
      pose_request.task_name = pose_ctl_task_name;
      pose_request.pose_type = request_code;
      pose_request.switch_x = switch_x;
      pose_request.switch_y = switch_y;
      pose_request.switch_th = switch_th;
      pose_request.x_set = x_set;
      pose_request.y_set = y_set;
      pose_request.th_set = th_set;
      pose_request.max_vel_x = max_vel_x;
      pose_request.max_vel_y = max_vel_y;
      pose_request.max_vel_th = max_vel_th;
      pose_request.precision_x = precision_x;
      pose_request.precision_y = precision_y;
      pose_request.precision_th = precision_th;

      pose_ctl_task_pub_.publish(pose_request);
        return true;
    }

/*****************************************************************************************************
 * Function:       // poseCtlTaskStatusInfoCallback
 * Description:    // 机器人位置调整完成事件回调
 * Subscriber:     // pose_ctl_task_status_info_sub
 * Topic:          // "pose_ctl_task_status_info"
 * Input:          // prosper_underpan::task_status_info::ConstPtr& msg: 自定义子任务消息
 * Output:         // none
 * Return:         // none
 * Notice:         // 机器人收到该消息后，即可认为机器人姿态已调整到位
*******************************************************************************************************/
    void ProsperUser::poseCtlTaskStatusInfoCallback(
        const prosper_underpan::task_status_info::ConstPtr& msg) {
        /*user code*/
        PoseAckInfo ack;
        ack.task_name = msg->task_name;
        ack.task_stamp = msg->time_stamp;
        ack.success = msg->status;
        ack.ack_code = msg->task_type;

        if (!pose_adjust_ack_callback_.empty()) {

            pose_adjust_ack_callback_(ack);
        }
    }

/*****************************************************************************************************
 * Function:       // selfDefTaskInfoCallback
 * Description:    // 机器人自定义事件回调
 * Subscriber:     // self_def_task_info_sub
 * Topic:          // "self_def_task_info"
 * Input:          // prosper_underpan::sub_task_info::ConstPtr& msg: 自定义子任务消息
 * Output:         // none
 * Return:         // none
 * Notice:         // 机器人收到该消息后，可用于用户自己的逻辑、算法代码的处理，一般来自调度系统
*******************************************************************************************************/
    void ProsperUser::selfDefTaskInfoCallback(const prosper_underpan::sub_task_info::ConstPtr& msg) {

//ROS_INFO("xxxxxxxxxxx:::%d",msg->task_type);
        if ((SubTaskType)msg->task_type == SUB_TASK_SELF_DEF) {

          YAML::Node node = YAML::Load(msg->self_def);
//ROS_INFO("CCCCCCCCC::%s",msg->self_def.c_str());

          if(node["task"].as<std::string>() == "set_getBucketArea"||node["task"].as<std::string>() == "set_putBucketArea"){
            if(!work_area_callbakc_.empty()) {

              if(node["type"].as<double>()==0){
                WorkArea work_area;
                tf::Vector3 p1;
                p1.setX(node["p1"][0].as<double>());
                p1.setY(node["p1"][1].as<double>());
                p1.setZ(0.0);
                tf::Vector3 p2;
                p2.setX(node["p2"][0].as<double>());
                p2.setY(node["p2"][1].as<double>());
                p2.setZ(0.0);
                tf::Vector3 p3;
                p3.setX(node["p3"][0].as<double>());
                p3.setY(node["p3"][1].as<double>());
                p3.setZ(0.0);
                tf::Vector3 p4;
                p4.setX(node["p4"][0].as<double>());
                p4.setY(node["p4"][1].as<double>());
                p4.setZ(0.0);
                tf::Vector3 p5;
                p5.setX(node["p5"][0].as<double>());
                p5.setY(node["p5"][1].as<double>());
                p5.setZ(0.0);

                work_area.buc_statue = node["buc_statue"].as<int>();
                work_area.task = node["task"].as<std::string>();
                work_area.seq = node["id"].as<int>();
                work_area.type = node["type"].as<int>();
                work_area.dock = p5;
                work_area.area.push_back(p1);
                work_area.area.push_back(p2);
                work_area.area.push_back(p3);
                work_area.area.push_back(p4);
                work_area_callbakc_(work_area);
              }
              if(node["type"].as<double>()==1){
                WorkArea work_area;
                tf::Vector3 p1;
                p1.setX(node["p1"][0].as<double>());
                p1.setY(node["p1"][1].as<double>());
                p1.setZ(0);
                tf::Vector3 p2;
                p2.setX(node["p2"][0].as<double>());
                p2.setY(node["p2"][1].as<double>());
                p2.setZ(0.0);
                //work_area.seq = node["id"].as<int>();
                work_area.task = node["task"].as<std::string>();
                work_area.type = node["type"].as<int>();
                work_area.dock = p1;
                work_area.area.push_back(p2);
                work_area_callbakc_(work_area);
              }

          }
            GroupControlInfo ack;
            ack.task_name = msg->self_def;
            ack.task_stamp = msg->time_stamp;
            ack.status = SUB_TASK_ACK_OK;
            sendGroupControlAck(ack);
         }else{
            if (!group_control_callback_.empty()) {
                GroupControlInfo ctl;
                ctl.evt_str = msg->self_def;
                ctl.task_name = msg->self_def;
                ctl.task_stamp = msg->time_stamp;

                group_control_callback_(ctl);
            }
          }
        }
    }

/*****************************************************************************************************
 * Function:       // SendGroupControlAck
 * Description:    // 机器人自定义事件完成状态响应
 *                 // 机器人每完成一个自定义子任务，都需要响应此消息，消息包括接收该任务时的时间戳和任务名字。
 * Publisher:      // task_status_pub
 * Topic:          // "task_status_info"
 * Input:          // const RobotDriverBase::GroupControlInfo& ack
 * Output:         // none
 * Return:         // 成功执行返回True
 * Notice:         // 用户响应时必须保证user_evt_str和接收到的字符串一样
*******************************************************************************************************/
    bool ProsperUser::sendGroupControlAck(const GroupControlInfo &ack) {

        prosper_underpan::task_status_info status_info;
        status_info.task_type = 5;
        status_info.time_stamp = ack.task_stamp;
        status_info.task_name = ack.task_name;
        status_info.status = ack.status;

        task_status_pub_.publish(status_info);

        return true;
    }

/*****************************************************************************************************
 * Function:       // emergentInfoCallBack
 * Description:    // 群控系统紧急消息回调函数
 *                 // 用户收到紧急消息后  添加自己的安全保护代码
 * Subscriber:     // emergent_info_sub
 * Topic:          // "emergent_info"
 * Input:          // const prosper_underpan::emergent_info::ConstPtr& msg 群控系统事件请求
 *                 // msg->cmd  0-取消任务 终止
 *                 // msg->cmd  1-中止任务 暂停
 *                 // msg->cmd  2-恢复任务 继续
 * Output:         // none
 * Return:         // none
 * Notice:         // 为了保证系统安全，用户请务必添加紧急情况下的处理源码
*******************************************************************************************************/


    void ProsperUser::emergentInfoCallBack(const prosper_underpan::emergent_info::ConstPtr& msg) {
        EmergentInfo info;
        if(!emergent_callback_.empty()){
            info.cmd = msg->cmd;
            emergent_callback_(info);
        }
    }
/***************************************************************************************************** 
 * Function:       // WriteTrailerStatusToYaml 
 * Description:    // 将机器人设置为拖车模式     
 * Input:          // is_trailer      是否将机器人处于拖车模式
 *                 // trailer_width   拖车宽度
 *                 // trailer_length  拖车长度
 * Output:         // none 
 * Return:         // none 
 * Notice:         // 如果退出拖车模式需要调用此函数，并且将is_trailer设置为false
*******************************************************************************************************/ 
void ProsperUser::WriteTrailerStatusToYaml(bool is_trailer,double trailer_width, double trailer_length){
    std::ofstream yaml_file("/tmp/TrailerStatus.yaml", std::ios::out | std::ios::binary);

    YAML::Emitter out(yaml_file);
    out << YAML::BeginMap;
    out << YAML::Key << "is_trailer" << YAML::Value << (int)is_trailer;
    out << YAML::Key << "trailer_width" << YAML::Value << trailer_width;
    out << YAML::Key << "trailer_length" << YAML::Value << trailer_length;
    out << YAML::EndMap;

    yaml_file.close();
}


    bool ProsperUser::serialCommunication(RobotDriverBase::OrderClass order){
      bool ok = false;
     // ROS_INFO("BBBBBBBBBBBBBBBB");
      switch(order){
        case PREPARECLOSE:{
         ROS_INFO("CCCCCCCCCC");
          char order[4] = {'1','A','L',0x0D};
          //order_type_ = 3;
          ok = armSend(order,sizeof(order),ERRORINQUIRE);
        //  ROS_INFO("OK:%d",ok);
          if(!ok){
             return false;
          }
          if(*(int*)data_flag_ != 0x30303030){
             ROS_INFO("error:%x",*(int*)data_flag_);
             return false;
          }
          char order1[5] = {'1','Q','X','1',0x0D};
          ok = armSend(order1,sizeof(order1),CONTROL);  //发送准备抓取指令,发送成功后开始后退
        }
        break;

        case STATUESIGAL:{
          char order[5] = {'1','R','L','1',0x0D};
          ok = armSend(order,sizeof(order),STATUEINQUIRE);
        }
        break;

        case SENDCLOSE:{
          char order[6] = {'1','R','L','1','4',0x0D};
          ok = armSend(order,sizeof(order),CONTROL);
        }
        break;

        case SENDEXPEND:{
          char order[6] = {'1','R','L','1','1',0x0D};
          ok = armSend(order,sizeof(order),CONTROL);
        }
        break;

        case STOPWORK:{
          char order[4] = {'1','S','K',0x0D};
          ok = armSend(order,sizeof(order),CONTROL);
        }
        break;
      }
      if(!ok){
         return false;
      }
      ROS_INFO("read is ok");
      return true;
    }

    bool ProsperUser::armSend(char order[],int size,int type){
        type_ = type;
        write_flag_ = true;
        recv_data_ = false;
        int timeout = 10;
        while(ros::ok()){
            //ROS_INFO("order=%x",*(int*)order);
            write(g_fdCom,order,size);
            timeout --;
            usleep(50000);

            if(recv_data_ == true){
               write_flag_ = false;
               recv_data_ = false;
               return true;
            }
            if(timeout < 0){
                ROS_INFO("SEND TIMEOUT");
                return false;
            }
        }
    }

    bool ProsperUser::armSendReset(char order[],int size){
        write(g_fdCom,order,size);
        usleep(50000);
        return true;
    }

    int ProsperUser::comOpen(void) {
        struct termios Opt;

        ROS_INFO("robot: comOpen() TODO --------------------");

        /* Open a UART */
        g_fdCom = open(serial_dev.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        if (g_fdCom <= 0) {
            ROS_INFO("robot: comOpen() %s open failed! ERROR Code = <%d>", "/dev/ttyUSB0", g_fdCom);
            return g_fdCom;
        } else
            ROS_INFO("robot: comOpen() %s open success.", "/dev/ttyUSB0");

        /* Configuration UART */
        tcgetattr(g_fdCom, &Opt);
        Opt.c_cflag &= ~CSIZE;
        Opt.c_cflag |= CS8;
        Opt.c_cflag &= ~PARENB;
        Opt.c_iflag &= ~INPCK;
        Opt.c_cflag &= ~CSTOPB;
        Opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
        Opt.c_oflag &= ~OPOST;
        tcflush(g_fdCom, TCIOFLUSH);
        Opt.c_cc[VTIME] = 0;
        Opt.c_cc[VMIN] = 0;
        cfsetispeed(&Opt, B9600);
        cfsetospeed(&Opt, B9600);
        tcsetattr(g_fdCom, TCSANOW, &Opt);

        ROS_INFO("robot: comOpen() DONE --------------------");
        ROS_INFO("comOpen done");
        return g_fdCom;
    }

   void ProsperUser::arm_working_exec(){
     ros::Rate loop_rate(50);
     comOpen();

     while(arm_working_ && ros::ok()){
       if(write_flag_ == false){
         data_flag_[0] = 0;
         data_flag_[1] = 0;
         data_flag_[2] = 0;
         data_flag_[3] = 0;
         usleep(50000);
         continue;
       }

       bytes_ = read(g_fdCom, read_data_, 9);


       if(bytes_!=0){
        // ROS_INFO("read:::%d",bytes_);
         for(int i=0;i<bytes_;i++){
           data_.push_back(read_data_[i]);
         }
         switch (type_){
           case 0:
             while(data_.size()>=2){
                 if(data_[0] != '1'||data_[1] !='%'){
                    data_.erase(data_.begin());
                 }
                 else {
                    data_.clear();
                    recv_data_ = true;
                    break;
                 }
             }
            break;
           case 1:
             while(data_.size()>=6){
                 if(data_[0] != '1'||data_[1] !='R'||data_[2] != 'L'||data_[3] !='1'||data_[4] != '='){
                     data_.erase(data_.begin());
                 }
                 else{
                     data_flag_[0] = data_[5];
                    // ROS_INFO("DDDD:%X",data_flag_[0]);
                     data_.clear();
                     recv_data_ = true;
                     break;
                 }
             }
            break;
           case 2:
             while(data_.size()>=8){
                 if(data_[0] != '1'||data_[1] !='A'||data_[2] != 'L'||data_[3] !='='){
                     data_.erase(data_.begin());
                 }
                 else{
                     data_flag_[0] = data_[4];
                     data_flag_[1] = data_[5];
                     data_flag_[2] = data_[6];
                     data_flag_[3] = data_[7];
                     data_.clear();
                     recv_data_ = true;
                     break;
                 }
             }
            break;
         }

       }
     loop_rate.sleep();
     }
   }
}

